function [dop,poss,Uvel] = DDop_position(Satpos,rcvpos,rcvvel,shangjie)
%INPUT 星历、测站位置速度、定位区间时长上界
%OUTPUT  DDOP、定位误差
%此函数只支持静态定位
% Satpos = [];%星历参数

global glovar;                              %全局变量
%% 参数配置
glovar.CLIGHT = 299792458;                  %光速 
glovar.file = 'starlink_like_720_36_53_9_eph.mat';                %STK文件
glovar.statname = 'BJFS';                   %测站名称 
glovar.sys = 1;                             % 1->单多普勒（4参数） 2->伪距+多普勒（5参数）
glovar.paranum = 5;                         % 待估参数4、5（位置参数+频漂+钟差） 暂时无效
% glovar.rcvblh = [-9 , 112.59 , 0]; %BLH（纬度经度高度（39.6086，115.8925，87.5））
% glovar.fc = 1625.5e06;                      %观测值频率1.6255GHZ
glovar.fc = 1.4e9;                      %starlink观测值频率11.325GHZ
glovar.MAXITR = 30;                         %最小二乘迭代次数
% 仿真策略-----------------------------------------------------------------
glovar.sigma_Freq = 0.1;                    %多普勒频移观测误差特性 HZ
glovar.sigma_Satpos = 2;                    %卫星星历误差方差
glovar.sigma_Satvel = 0.05;                    %卫星速度误差方差(取值过大，数cm/s比较好)
glovar.sigma_PrangO = 1;                    %伪距观测误差方差
glovar.delta_Dt =  1e-6;                    %用户钟差
glovar.delta_Fu =  10;                      %默认用户频差
glovar.mode = 1;                            %定位模式（1->多星，2->单星）
glovar.intime = 1;                           %积分定位时长，取1时为瞬时多普勒定位
glovar.satprn = 38;                         %单星模式下选取目标星

% 测站真实信息-------------------------------------------------------------
rcvpos = llh2xyz(rcvpos);
% rcvvel = [10,10,10];                                       %测站速度

% 截取时间（默认长度600s）--------------------------------------------------
% shangjie = 1600;
timespan = [1,shangjie];     %38星（第344s卫星可见）
% timespan = [439,1339];   %32星（第439s卫星可见）

% 根据选择模式确定图片名称 -------------------------------------------------
if glovar.mode==1 %判断单星和多星
    if glovar.sys ==1 %多普勒定位
        figurename=['Doppler Location(MS) ',glovar.statname]; %图片输出名
        titlename = 'Starlink Doppler Position(Integrate)';    %图标题名
    else
        figurename=['Doppler+Prang Location(MS) ',glovar.statname]; %图片输出名
        titlename = 'Iridrum Doppler+Prang Position(Multi-Sat)';    %图标题名
    end

else
    if glovar.sys ==1 %多普勒定位
        figurename=['Doppler Location(SS) ',statname];
        titlename = 'Iridrum Doppler Position(Single-Sat)';
    else
        figurename=['Doppler+Prang Location(SS) ',glovar.statname]; %图片输出名
        titlename = 'Iridrum Doppler+Prang Position(Single-Sat)';    %图标题名
    end
end
%% 读取文件
% Satpos = readSTKfile(glovar.file); %星历文件
% sc_file = "D:\Working_space\Course_pro\src2\satllite_trans\starlink_like_720_36_53_9_eph.mat";
% load(glovar.file,'Satpos');
Tu = readclkfile;           %钟速文件（频差）
%% 多普勒/伪距观测值仿真
[DopplerOBS,PrangOBS,VisibleSat]=SimDoppler(rcvpos,rcvvel,Satpos,timespan,Tu);
%% 多普勒定位
[num,~] = size(DopplerOBS);                %历元长度
% 变量初始化
Satpos = [];Satvel = [];Satel  = [];     %多历元卫星位置、速度、高度角
DopObs = [];                             %多普勒观测值积累
CompareResult=[];DCompareResult=[];
matrixDOP_Result=[];DmatrixDOP_Result=[];
EPosState = [];DEPosState=[];
Uvel = [];
Initpos = 0.99*[rcvpos,0]; %确定初始参数
% Initpos2 = deter_Initpos(rcvpos);
for ti =1:num  %历元循环  
    snum = VisibleSat(ti).vsatnum;  %单历元卫星数
    PrangObs= []; %单历元伪距   
    esatpos = []; %单历元所有可见卫星位置
    esstvel = []; %单历元所有可见卫星速度
    edopobs = []; %单历元所有多普勒观测值
    eprangobs=[]; %单历元所有伪距观测值
    esatel =  []; %单历元所有可见卫星高度角   
    
% 多普勒观测值多历元积累
    edopobs = DopplerOBS(ti,:)';
    index = find(edopobs==0);
    edopobs(index)=[];  %将单历元中多普勒0值置空
%     DopObs = [edopobs]; %多历元多普勒观测值积累，一个列向量
% 伪距观测值单历元替换
    eprangobs = PrangOBS(ti,:)';
    index = find(eprangobs==0);
    eprangobs(index)=[];  
    PrangObs = eprangobs; %单历元伪距观测值向量 
% 单历元卫星循环，获取单个历元所有卫星位置速度     
    for si =1:snum  
       osatpos = VisibleSat(ti).sat(si).rs(1:3);
       osatvel = VisibleSat(ti).sat(si).rs(4:6);
       osatel  = VisibleSat(ti).sat(si).el;
       
       esatpos = [esatpos;osatpos];%snum*3的卫星坐标矩阵
       esstvel = [esstvel;osatvel];%snum*3的卫星速度矩阵
       esatel  = [esatel ;osatel];%snum*1的仰角向量
    end
% 卫星位置速度多历元积累（用于多普勒定位）    
    Satpos = [Satpos;esatpos];
    Satvel = [Satvel;esstvel];
    Satel = [Satel;esatel];
    DopObs = [DopObs;edopobs]; %多历元多普勒观测值积累，一个列向量
% 单独给伪距提供单历元位置速度
    PrangObs = [PrangObs,esatpos] ;%[伪距，卫星位置]
    
    [sm,sn] = size(Satpos);   
    [smlim,Cresultdebug]=vardebug;% 根据不同定位模式和待估参数个数确定参数默认值
%可用数据小于一定数量时不能解算
    if sm<smlim %多普勒为4  伪距+多普勒为3
        CompareResult = [CompareResult;Cresultdebug];
        matrixDOP_Result = [matrixDOP_Result;Cresultdebug];
        EPosState = [EPosState;-1];
        continue        
    end  
%% FOA定位解算
        if ~mod(ti,glovar.intime)
            [PosSol_static,PosState_static,matrixDOP] = FOA(rcvvel,Satpos,Satvel,DopObs,PrangObs,Initpos);%Initpos用于位置迭代
            Uvel = [Uvel;Velops(DopObs,Satvel,PosSol_static,Satpos)];%多普勒定速
            Satpos = [];
            Satvel = [];
            DopObs = [];
        else
            continue;
        end

    if PosState_static ==1   %定位解算成功 
        Initpos = PosSol_static; %上次定位结果作为下次定位初始坐标
        if glovar.sys == 1 % 多普勒定位 
            CompareResult = [CompareResult;(PosSol_static(1:4)-[rcvpos,Tu(ti)])];
            matrixDOP_Result = [matrixDOP_Result;[matrixDOP(1,1),matrixDOP(2,2),matrixDOP(3,3),matrixDOP(4,4)]];
            EPosState = [EPosState;PosState_static];
        else  %伪距+多普勒定位   
            CompareResult = [CompareResult;(PosSol_static(1:5)-[rcvpos,Tu(ti),glovar.delta_Dt*glovar.CLIGHT])];
            matrixDOP_Result = [matrixDOP_Result;[matrixDOP(1,1),matrixDOP(2,2),matrixDOP(3,3),matrixDOP(4,4),matrixDOP(5,5)]];
            EPosState = [EPosState;PosState_static];
        end
    else  %定位解算失败
        if glovar.sys == 1 % 多普勒定位 
            CompareResult = [CompareResult;Cresultdebug];
            matrixDOP_Result = [matrixDOP_Result;[matrixDOP(1,1),matrixDOP(2,2),matrixDOP(3,3),matrixDOP(4,4)]];
            EPosState = [EPosState;PosState_static];
        else  %伪距+多普勒定位   
            CompareResult = [CompareResult;Cresultdebug];
            matrixDOP_Result = [matrixDOP_Result;[matrixDOP(1,1),matrixDOP(2,2),matrixDOP(3,3),matrixDOP(4,4),matrixDOP(5,5)]];
            EPosState = [EPosState;PosState_static];
        end
    end  
    % disp("%"+num2str(ti));
end
%% 统计
    CompareResult = CompareResult(~any(isnan(CompareResult), 2), :);%去掉因参数不够解算历元而出现的NaN值
    matrixDOP_Result = matrixDOP_Result(~any(isnan(matrixDOP_Result), 2), :);
    for i =1:floor(num/glovar.intime)
        dop(i,1) = sqrt(matrixDOP_Result(i,1)+matrixDOP_Result(i,2)+matrixDOP_Result(i,3));
        poss(i,1)= sqrt(CompareResult(i,1)^2+CompareResult(i,2)^2+CompareResult(i,3)^2);
    end  
    for i =1:floor(num/glovar.intime)
       diff(i,1) = dop(i,1)- poss(i,1);    
    end
        

    x = 0:glovar.intime:shangjie-1;
%% 统计结果绘图
if glovar.mode==1 %判断单星和多星
    nn = -30:10:30;
    ylimnum = [-30  30];
else
    nn = -1000:200:1000;
    ylimnum = [-1000  1000];
end

x = x';

n = 0:20:shangjie;
n = n';
nn =nn';

figure
grid on
hold on
set(gca,'linewidth',1.5);
set(gca,'GridAlpha',0.5,'GridLineStyle','--');
set(gca,'Box','on');
set(gca,'FontSize',13);
set(gca,'xtick',n,'xticklabel',num2str(n));
set(gca,'ytick',nn,'yticklabel',num2str(nn));
hold on 
charname1=['X';'Y';'Z'];
% epsilon = ['Error x','Error y','Error z'];
% epsilon = strjoin(epsilon);
for i =1:3
    plot(x,CompareResult(:,i),'linewidth',2);
    hold on  
end
epsilon_x = mean(abs(CompareResult(:,1)));
epsilon_y = mean(abs(CompareResult(:,2)));
epsilon_z = mean(abs(CompareResult(:,3)));
legend(['\epsilon','x','=',num2str(epsilon_x,'%8.3f'),'m'],['\epsilon','y','=',num2str(epsilon_y,'%8.3f'),'m'],['\epsilon','z','=',num2str(epsilon_z,'%8.3f'),'m']);
ylim(ylimnum);
xlim([0 num]);
xlabel('TimeSpan/s');
ylabel('m');
title(titlename,'fontsize',15);
end

